#include <dos.h>
#include <stdlib.h>

#include "mpx_supt.h"
#include "pqueue.h"
#include "R2.h"
#include "R3.h"

#define SYS_STACK_SIZE 1024

/****************************** Global Variables ******************************/
unsigned char sys_stack[SYS_STACK_SIZE];
unsigned short ss_save = NULL;
unsigned short sp_save = NULL;
unsigned short new_ss = NULL;
unsigned short new_sp = NULL;
ppcb cop = NULL;

context *context_ptr;
params  *param_ptr;

int dispatch_error;
int sys_len;

extern pq ready, blocked, suspended, suspended_blocked;

void test1_R3();
void test2_R3();
void test3_R3();
void test4_R3();
void test5_R3();

/**************************Function Implmentation******************************/
void interrupt sys_call() {
	/* Save the current stack information to the current operating process's
	 * stack.
	 */
	cop->stack_top = MK_FP(_SS, _SP);
	// Decrement stack_top to point to new empty?
	
	/* Switch to sys_call's stack.
	 */
	new_ss =  FP_SEG(sys_stack);
	new_sp =  FP_OFF(sys_stack);
	new_sp += SYS_STACK_SIZE;
	_SS    =  new_ss;
	_SP    =  new_sp;
	
	/* Extract the context and parameters from the stack.
	 */
	context_ptr = (context *) cop->stack_top;
	param_ptr   = (params *) (cop->stack_top + sizeof(context));
	
	/* Either put the COP back in the ready queue or kill it, depending upon the
	 * process's request.
	 */
	if(param_ptr->op_code == IDLE){
		Insert_PCB(READY, cop);
	}
	else if(param_ptr->op_code == EXIT){
		sys_len = 30;
		sys_req(WRITE, TERMINAL, "Received termination request.\n", &sys_len);
		Free_PCB(cop);
	}
	
	/* Save the result of the operation into the COP's result register, clean up
	 * for a dispatch, then attempt to dispatch the next process.
	 */
	context_ptr->AX = OK;
	cop = NULL;
	dispatch();
}

void interrupt dispatch(){
	if(ss_save == NULL){
		ss_save = _SS;
		sp_save = _SP;
	}
	if((dispatch_error = pq_dequeue(&ready, (void**)&cop)) == OK){
		new_ss = FP_SEG(cop->stack_top);
		new_sp = FP_OFF(cop->stack_top);
		_SS = new_ss;
		_SP = new_sp;
	}
	else if(dispatch_error == ERR_PQUEUE_NULLARG){
		sys_len = 51;
		sys_req(
			WRITE,
			TERMINAL,
			"\n\tError: Invalid argument passed to pq_dequeue.\n\n",
			&sys_len
		);
		sys_exit();
	}
	else{
		cop = NULL;
		_SS = ss_save;
		_SP = sp_save;
		ss_save = NULL;
		sp_save = NULL;
	}		
}

void Load_Procs(int argc, char **argv) {
	int i;
	
	//*
	void(*testFunctions[5])()= {
		(void(*)()) test1_R3,
		(void(*)()) test2_R3,
		(void(*)()) test3_R3,
		(void(*)()) test4_R3,
		(void(*)()) test5_R3
	};
	
	char *testNames[] = {
		"testproc1",
		"testproc2",
		"testproc3",
		"testproc4",
		"testproc5"
	};
	//*/
	
	/*
	void (*testFunctions[1])() = { (void(*)()) test1_R3 };
	char *testNames[] = { "testproc1" };
	//*/
	
	argc = argc;
	argv = argv;
	
	for(i = 0; i < 5; i++){
		ppcb np;
		context *npc;
		np = Setup_PCB(testNames[i], APPLICATION, 0);
		npc = (context*) np->stack_top;
		npc->IP = FP_OFF(testFunctions[i]); 
		npc->CS = FP_SEG(testFunctions[i]);
		npc->FLAGS = 0x200;
		npc->DS = _DS;
		npc->ES = _ES;
		Insert_PCB(READY, np);
	}
	
}

void Dispatch(int argc, char **argv){
	argc = argc;
	argv = argv;
	
	if(pq_is_empty(&ready) == TRUE){
		int len = 22;
		sys_req(WRITE, TERMINAL, "Ready queue is empty.", &len);
	}
	else{
		dispatch();
	}
}